#include "TestSketch.h"
#include "Servo.h"
#include "LiquidCrystal.h"

#define PIN_MOTOR_DRIVE 9
#define SPEED_MOTOR_OFF 96
#define DELAY_MOTOR_OFF 0
#define SPEED_MOTOR_LOW 110
#define DELAY_MOTOR_LOW 10
#define SPEED_MOTOR_HIGH 130
#define DELAY_MOTOR_HIGH 150

#define PIN_SERVO_CLUTCH 10
#define DEGREE_CLUTCH_DOWN 57
#define DELAY_CLUTCH_LOW 10
#define DEGREE_CLUTCH_UP 180

#define SECONDS_FULL_THROTTLE 3
#define SECONDS_IDLE 0

#define LOAD_WHEEL_MIN_RPM 100
#define BUTTON_DEBOUNCE 90

#define ANALOG_IN_PIN 0
#define AVERAGE_DELAY 800
#define PIN_MIN 0
#define PIN_MAX 1023
#define VOLTS_MIN 0.00
#define VOLTS_MAX 17.06

LiquidCrystal lcd( 12, 11, 5, 4, 3, 7 );

Servo drive;
Servo clutch;

struct tach
{
  unsigned long nMillisLast;
  unsigned long nMillisCurrent;
};

struct tach tachLoadWheel;

void tachInit( struct tach &tach, int nMillisLastInit, int nMillisCurrentInit )
{
  tach.nMillisLast = nMillisLastInit;
  tach.nMillisCurrent = nMillisCurrentInit;
}

void tachISR()
{
  if ( ( millis() - tachLoadWheel.nMillisCurrent ) >= BUTTON_DEBOUNCE )
  {
    tachLoadWheel.nMillisLast = tachLoadWheel.nMillisCurrent;
    tachLoadWheel.nMillisCurrent = millis();
  }
}

int tachGetRpm( struct tach &tach )
{
  unsigned long nDiff = ( tach.nMillisCurrent - tach.nMillisLast );
  return ( 60000 / nDiff );
}

double pinToVolts( int nPinVal )
{
  double nReturn = ( ( (double) ( VOLTS_MAX - VOLTS_MIN ) / (double) ( PIN_MAX - PIN_MIN ) ) * (double) nPinVal );

  return nReturn;
}

double getAverage()
{
  double nReturn = 0;

  int nVal1 = 0;
  lcd.setCursor( 0, 1 );
  lcd.print( "reading 1: " );
  nVal1 = analogRead( ANALOG_IN_PIN );
  lcd.print( nVal1 );
  delay( AVERAGE_DELAY );

  int nVal2 = 0;
  lcd.setCursor( 0, 1 );
  lcd.print( "reading 2: " );
  nVal2 = analogRead( ANALOG_IN_PIN );
  lcd.print( nVal2 );
  delay( AVERAGE_DELAY );

  int nVal3 = 0;
  lcd.setCursor( 0, 1 );
  lcd.print( "reading 3: " );
  nVal3 = analogRead( ANALOG_IN_PIN );
  lcd.print( nVal3 );
  delay( AVERAGE_DELAY );

  int nVal4 = 0;
  lcd.setCursor( 0, 1 );
  lcd.print( "reading 4: " );
  nVal4 = analogRead( ANALOG_IN_PIN );
  lcd.print( nVal4 );
  delay( AVERAGE_DELAY );

  int nVal5 = 0;
  lcd.setCursor( 0, 1 );
  lcd.print( "reading 5: " );
  nVal5 = analogRead( ANALOG_IN_PIN );
  lcd.print( nVal5 );
  delay( AVERAGE_DELAY );

  int nAvgRaw = ( ( nVal1 + nVal2 + nVal3 + nVal4 + nVal5 ) / 5 );
  nReturn = pinToVolts( nAvgRaw );

  lcd.clear();
  lcd.print( "get batt level" );
  lcd.setCursor( 0, 1 );
  lcd.print( "final avg: " );
  lcd.print( nReturn );

  return nReturn;
}

void setup()
{
  lcd.begin( 16, 2 );
  lcd.print( "Initializing..." );

  attachInterrupt( 0, tachISR, FALLING );
  digitalWrite( 2, HIGH );

  analogReference( INTERNAL );

  analogRead( ANALOG_IN_PIN ); //after setting the analog reference, the first few analog reads will be false values
  analogRead( ANALOG_IN_PIN ); //these get rid of it by repeatedly reading the analog pin
  analogRead( ANALOG_IN_PIN );
  analogRead( ANALOG_IN_PIN );
  analogRead( ANALOG_IN_PIN );
  analogRead( ANALOG_IN_PIN );
  analogRead( ANALOG_IN_PIN );
  analogRead( ANALOG_IN_PIN );
  analogRead( ANALOG_IN_PIN );
  analogRead( ANALOG_IN_PIN );
  analogRead( ANALOG_IN_PIN );
  analogRead( ANALOG_IN_PIN );
  analogRead( ANALOG_IN_PIN );
  analogRead( ANALOG_IN_PIN );
  analogRead( ANALOG_IN_PIN );
  analogRead( ANALOG_IN_PIN );

  tachInit( tachLoadWheel, 0, 0 );

  Serial.begin( 115200 );

  Serial.println( "Initializing....." );

  drive.attach( PIN_MOTOR_DRIVE );
  clutch.attach( PIN_SERVO_CLUTCH );

  drive.write( SPEED_MOTOR_OFF );
  clutch.write( DEGREE_CLUTCH_UP );

  delay( 5000 );
  Serial.println( "Initialization complete" );
  lcd.clear();
}

void loop()
{

  lcd.clear();
  lcd.print( "up and low" );
  Serial.println( "up and low" );
  drive.write( SPEED_MOTOR_LOW );
  clutch.write( DEGREE_CLUTCH_UP );
  delay( 3000 );

  lcd.clear();
  lcd.print( "down and low" );
  Serial.println( "down and low" );
  for ( int nPos = DEGREE_CLUTCH_UP; nPos >= DEGREE_CLUTCH_DOWN; nPos-- )
  {
    clutch.write( nPos );
    Serial.print( "clutch pos:  " );
    Serial.println( nPos );
    delay (DELAY_CLUTCH_LOW);
  }
  delay( 1000 );

  lcd.clear();
  lcd.print( "keep low" );
  Serial.println( "keep low" );
  delay( SECONDS_IDLE * 1000 );

  lcd.clear();
  lcd.print( "down and high" );
  Serial.println( "down and high" );
  for ( int nPos = SPEED_MOTOR_LOW; nPos <= SPEED_MOTOR_HIGH; nPos++ )
  {
    drive.write( nPos );
    Serial.print( "motor speed:  " );
    Serial.println( nPos );
    delay( DELAY_MOTOR_HIGH );
  }

  lcd.clear();
  lcd.print( "keep high" );
  Serial.println( "keep high" );
  delay( SECONDS_FULL_THROTTLE * 1000 );

  lcd.clear();
  lcd.print( "up and off" );
  Serial.println( "up and off" );
  drive.write( SPEED_MOTOR_OFF );
  clutch.write( DEGREE_CLUTCH_UP );
  delay( 3000 );

  lcd.clear();
  lcd.print( "get batt level" );
  getAverage();
  delay( 3000 );

  lcd.clear();
  lcd.print( "wait for slow" );
  Serial.println( "wait for slow" );
  delay( 2000 );
//  while ( tachGetRpm( tachLoadWheel ) > LOAD_WHEEL_MIN_RPM )
//  {
//    if ( millis() % 10 == 0 )
//    {
//      lcd.clear();
//      lcd.print( "rpm: " );
//      lcd.print( tachGetRpm( tachLoadWheel ) );
//    }
//  }
  Serial.println( "min rpm reached" );

  Serial.println( "" );
  Serial.println( "" );
  Serial.println( "" );
  Serial.println( "" );
  Serial.println( "" );
  Serial.println( "" );
  Serial.println( "" );
  Serial.println( "" );
  Serial.println( "" );
  Serial.println( "" );

}
